#include "./cells.h"
#include <math.h>

#if defined(_OPENMP)
#include <omp.h>
#endif

size_t mds_cells_count1( Real *Lc, const Real L, const Real sigma_c )
{
	size_t Nc;
	assert( Lc != NULL );
	assert( L > 0      );
	assert( sigma_c > 0    );
	Nc = (size_t) floor( L/sigma_c );
	if( Nc < 1) Nc = 1;
	*Lc = L / Nc;
	return Nc;
}

size_t mds_cells_count( mds_uvector *cells_per_dimension, mds_vector *cells_size, const mds_vector *box, const Real sigma_c )
{
	size_t ans = 1;
	assert( cells_per_dimension);
	assert( cells_size);
	assert( box );
	assert( sigma_c > 0 );
	
	ans *= ( cells_per_dimension->x = mds_cells_count1( & cells_size->x, box->x, sigma_c ) );
	ans *= ( cells_per_dimension->y = mds_cells_count1( & cells_size->y, box->y, sigma_c ) );
	ans *= ( cells_per_dimension->z = mds_cells_count1( & cells_size->z, box->z, sigma_c ) );
	
	return ans;
}

#include "../sys/arrays.h"

int   mds_cells_load( mds_cells *cells, const mds_vector *box, const Real r_c )
{
	assert( cells );
	assert( box   );
	assert( r_c > 0);
	memset(cells,0,sizeof(mds_cells));
	cells->count = mds_cells_count( & cells->num, & cells->length, box, r_c );
	cells->array = (mds_cell ***)mds_array3d_create( cells->num.x, cells->num.y, cells->num.z, sizeof(mds_cell));
	if( NULL == cells->array )
	{
		memset( cells, 0, sizeof(mds_cells) );
		return -1;
	}
	
	fprintf( stderr, "@@ Box for cells: [ %g %g %g]\n", box->x, box->y, box->z );
	cells->offset.x = box->x/2;
	cells->offset.y = box->y/2;
	cells->offset.z = box->z/2;
	
	cells->factor.x = (Real)(1) / cells->length.x;
	cells->factor.y = (Real)(1) / cells->length.y;
	cells->factor.z = (Real)(1) / cells->length.z;
	
	{
		register size_t i=0;
		mds_cell *pCell  = & cells->array[0][0][0];
		for(i=0;i<cells->count;++i)
		{
			pCell[i].label = i;
		}
	}
	
	{
		fprintf( stderr, "######## #Cells = %u\n", (unsigned)(cells->count)); 
#if defined(_OPENMP)
		fprintf( stderr, "######## Setting Up MultiThreading\n");
		cells->level = omp_get_max_threads();
		fprintf( stderr, "######## Level= %u\n", (unsigned)(cells->level) );
		if( cells->level > MDS_THREAD_MAX )
		{
			fprintf( stderr, "<<Catastrophic Error: too many threads !!!>>\n");
			exit(-1);
		}
		else {
			size_t        todo = cells->count;
			size_t        rank;
			const  size_t size = cells->level;
			size_t        ipos = 0;
			
			for( rank=0; rank < size; ++rank )
			{
				const size_t width = todo/(size-rank);
				cells->enter[rank] = ipos;
				cells->leave[rank] = ipos+width;
				fprintf( stderr, "######## Core #%u: @%u + %u (<%u)\n", (unsigned) rank, (unsigned) ipos, (unsigned) width, (unsigned) (ipos+width) );
				todo -= width;
				ipos += width;
			}
			
		}
#else
		/** one thread **/
		fprintf( stderr, "######## Sequential Layout!!\n");
		cells->level = 1;
		cells->enter[0] = 0;
		cells->leave[0] = cells->count;
#endif
		
	}
	return 0;
}

void mds_cells_quit( mds_cells *cells )
{
	if( cells && cells->array )
	{
		mds_array3d_delete( (void ***)(cells->array), cells->num.x, cells->num.y, cells->num.z, sizeof(mds_cell));
		memset( cells, 0, sizeof(mds_cells) );
	}
	
}

static int compare_cells( const void *lhs, const void *rhs )
{
	const mds_cell **ppL = (const mds_cell **)lhs;
	const mds_cell **ppR = (const mds_cell **)rhs;
	const ptrdiff_t L = (*ppL)->label;
	const ptrdiff_t R = (*ppR)->label;
	/*fprintf( stderr, "%d/%d\n", L, R );*/
	return L-R;
}

static 
void mds_order_neighbors( mds_cells *cells )
{
	mds_cell       *pCell = & cells->array[0][0][0];
	const size_t    nCell = cells->count;
	register size_t i;
	for( i=0; i < nCell; ++i, ++pCell )
	{
		mds_cell **neighbors = pCell->neighbors;
#if 0
		register size_t j;
		fprintf(stderr, "Cell #%u\n", pCell->label);
		for( j=0; j < pCell->num_neighbors; ++j )
		{
			fprintf( stderr, " %u", neighbors[j]->label );
		}
		fprintf( stderr, "\n");
#endif
		qsort(neighbors, pCell->num_neighbors, sizeof(mds_cell *), compare_cells);
#if 0
		for( j=0; j < pCell->num_neighbors; ++j )
		{
			fprintf( stderr, " %u", neighbors[j]->label );
		}
		fprintf( stderr, "\n");
#endif
	}
}

static
size_t mds_link_to( mds_cell *cell, mds_cell *other )
{
	register size_t i;
	assert( cell  != NULL );
	assert( other != NULL );
	assert( other != cell );
	assert( cell->num_neighbors < MDS_CELL_MAX_NEIGHBORS);
	
	for( i=0; i < cell->num_neighbors; ++i )
	{
		const mds_cell *scan = cell->neighbors[i];
		assert( scan != NULL );
		assert( scan != cell );
		if( scan == other )
			return 0;
	}
	
	cell->neighbors[ cell->num_neighbors++ ] = other;
	return 1;
}


size_t  mds_cells_link( mds_cells *cells, const mds_uvector *pbc )
{
	assert( cells );
	assert( cells->array );
	assert( cells->num.x > 0 );
	assert( cells->num.y > 0 );
	assert( cells->num.z > 0 );
	assert( pbc   );
	{
		const size_t nx    = cells->num.x;
		const size_t ny    = cells->num.y;
		const size_t nz    = cells->num.z;
		const size_t nxm   = nx-1;
		const size_t nym   = ny-1;
		const size_t nzm   = nz-1;
		const int    link_i = nx>1;
		const int    link_j = ny>1;
		const int    link_k = nz>1;
		mds_cell  ***array = cells->array;
		register size_t i,j,k;
		size_t       links = 0;
		
		for( i=0; i < nx; ++i )
		{
			const size_t im      = (i+nxm) % nx;
			const size_t ip      = (i+1)   % nx;
			const int    link_im = link_i && (i>0  ||pbc->x);
			const int    link_ip = link_i && (i<nxm||pbc->x);
			
			for( j=0; j < ny; ++j )
			{
				const size_t jm = (j+nym) % ny;
				const size_t jp = (j+1)   % ny;
				const int    link_jm = link_j && (j>0   || pbc->y);
				const int    link_jp = link_j && (j<nym || pbc->y);
				
				for( k=0; k < nz; ++k )
				{
					const size_t km   = (k+nzm) % nz;
					const size_t kp   = (k+1)   % nz;
					const int    link_km = link_k && (k>0   || pbc->z);
					const int    link_kp = link_k && (k<nzm || pbc->z);
					mds_cell    *cell = &array[i][j][k];
					cell->num_neighbors = 0;
#define MDS_LINK_CELL(a,b,c) \
/*	*/	do { \
/*	*/		assert( &array[a][b][c] != cell ); \
/*	*/		links += mds_link_to(cell,&array[a][b][c]);\
/*	*/	} while(0)
					
					/** im **/
					if( link_im && link_jm && link_km ) MDS_LINK_CELL(im,jm,km);
					if( link_im && link_jm )            MDS_LINK_CELL(im,jm,k);
					if( link_im && link_jm && link_kp ) MDS_LINK_CELL(im,jm,kp);
					
					if( link_im && link_km)             MDS_LINK_CELL(im,j,km);
					if( link_im)                        MDS_LINK_CELL(im,j,k);
					if( link_im && link_kp)             MDS_LINK_CELL(im,j,kp);
					
					if( link_im && link_jp && link_km)  MDS_LINK_CELL(im,jp,km);
					if( link_im && link_jp)             MDS_LINK_CELL(im,jp,k);
					if( link_im && link_jp && link_kp)  MDS_LINK_CELL(im,jp,kp);
					
					/** i **/
					if( link_jm && link_km )            MDS_LINK_CELL(i,jm,km);
					if( link_jm)                        MDS_LINK_CELL(i,jm,k);
					if( link_jm && link_kp )            MDS_LINK_CELL(i,jm,kp);
					
					if( link_km )                       MDS_LINK_CELL(i,j,km);
					/** MDS_LINK_CELL(i,j,k); **/
					if( link_kp )                       MDS_LINK_CELL(i,j,kp);
					
					if( link_jp && link_km )            MDS_LINK_CELL(i,jp,km);
					if( link_jp )                       MDS_LINK_CELL(i,jp,k);
					if( link_jp && link_kp )            MDS_LINK_CELL(i,jp,kp);
					
					/** ip **/
					if( link_ip && link_jm && link_km ) MDS_LINK_CELL(ip,jm,km);
					if( link_ip && link_jm )            MDS_LINK_CELL(ip,jm,k);
					if( link_ip && link_jm && link_kp ) MDS_LINK_CELL(ip,jm,kp);
					
					if( link_ip && link_km )            MDS_LINK_CELL(ip,j,km);
					if( link_ip )                       MDS_LINK_CELL(ip,j,k);
					if( link_ip && link_kp )            MDS_LINK_CELL(ip,j,kp);
					
					if( link_ip && link_jp && link_km ) MDS_LINK_CELL(ip,jp,km);
					if( link_ip && link_jp )            MDS_LINK_CELL(ip,jp,k);
					if( link_ip && link_jp && link_kp ) MDS_LINK_CELL(ip,jp,kp);
					
				}
			}
		}
		
		/** ordering cells **/
		mds_order_neighbors(cells);
		return links;
	}
}


#define MDS_CELL_INDEX(Q) ( (long)(floor((r->Q+offset.Q)*factor.Q)) ) 

void   mds_cells_dispatch( mds_cells        *cells, 
						  const mds_vector  *positions,
						  mds_pnode         *particle_nodes, 
						  size_t             num_particles)
{
	assert( cells );
	assert( cells->array );
	assert( cells->num.x > 0 );
	assert( cells->num.y > 0 );
	assert( cells->num.z > 0 );
	assert( positions      != NULL );
	assert( particle_nodes != NULL );
	{
		mds_cell       ***array  = cells->array;
		const size_t      count  = cells->count;
		mds_cell         *base   = &array[0][0][0];
		const mds_vector  offset = cells->offset;
		const mds_vector  factor = cells->factor;
		const mds_uvector num    = cells->num; 
		const long        num_x = num.x;
		const long        num_y = num.y;
		const long        num_z = num.z;
		register size_t   m;
		
		/** initialize each cell list **/
		for( m=0; m < count; ++m ) base[m].first = NULL;
		
		/** dispatch all particles **/
		for( m=0; m < num_particles; ++m )
		{
			/** get position and associated particle_node **/
			const mds_vector  *r    = &positions[m];
			mds_pnode         *node = &particle_nodes[m]; 
			
			/** compute indices **/
			long i = MDS_CELL_INDEX(x); 
			long j = MDS_CELL_INDEX(y);
			long k = MDS_CELL_INDEX(z);
			
			
			/*fprintf( stderr, "<%ld %ld %ld> [%g %g %g]\n", i,j,k, r->x, r->y, r->z);*/
			assert( i <= num_x ); assert(i>=-1);
			assert( j <= num_y ); assert(i>=-1);
			assert( k <= num_z ); assert(i>=-1);
			
			if( i >= num_x ) --i; else { if(i<0) ++i; }
			if( j >= num_y ) --j; else { if(j<0) ++j; }
			if( k >= num_z ) --k; else { if(k<0) ++k; }
			
			assert( node->id == m );
			
			/** push node in the right cell **/
			mds_cell *cell = & array[i][j][k];
			node->next     = cell->first;
			cell->first    = node;
		}
	}
	
}


